﻿#include"Sfjps.h"


//判断邻居类型，是否是最佳邻居和强迫邻居
//入参：单元地图8位二进制格式(十进制范围0-255)，父节点位置(0-8)、检测的邻居的位置(0-8)
//当前点在单元地图的中心（4）位置
bool* Sfjps::Prune(short unitMap, char p, char n) {
    static bool retType[2];//返回的类型
    char obstaclePos = 0;

#if 0
    //单元地图预处理
    char unitMapTmp = 0;
    if (p == 0) {//单元地图顺时针转180度
        unitMapTmp = unitMapTmp | (unitMap << 8 & (1 << 8));//0->8
        unitMapTmp = unitMapTmp | (unitMap << 6 & (1 << 7));//1->7
        unitMapTmp = unitMapTmp | (unitMap << 4 & (1 << 6));//2->6
        unitMapTmp = unitMapTmp | (unitMap << 2 & (1 << 5));//3->5
        unitMapTmp = unitMapTmp | (unitMap >> 2 & (1 << 3));//5->3
        unitMapTmp = unitMapTmp | (unitMap >> 4 & (1 << 2));//6->2
        unitMapTmp = unitMapTmp | (unitMap >> 6 & (1 << 1));//7->1
        unitMapTmp = unitMapTmp | (unitMap >> 8 & (1 << 0));//8->0
        p = 8;
    }
#endif // 0

    if (p == 0) {
        if (n == 7 || n == 5 || n == 8) {
            retType[0] = true;
            retType[1] = false;
        }

        if (n == 2) {
            obstaclePos = unitMap >> 1 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 6) {
            obstaclePos = unitMap >> 3 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }

        if (n == 1 || n == 3) {
            retType[0] = false;
            retType[1] = false;
        }
    }

    if (p == 1) {
        if (n == 7) {
            retType[0] = true;
            retType[1] = false;
        }
        if (n == 6) {
            obstaclePos = unitMap >> 3 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 8) {
            obstaclePos = unitMap >> 5 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }

        if (n == 0 || n == 2 || n == 3 || n == 5) {
            retType[0] = false;
            retType[1] = false;
        }
    }

    if (p == 2) {
        if (n == 7 || n == 6 || n == 3) {
            retType[0] = true;
            retType[1] = false;
        }
        if (n == 0) {
            obstaclePos = unitMap >> 1 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 8) {
            obstaclePos = unitMap >> 5 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }

        if (n == 1 || n == 5) {
            retType[0] = false;
            retType[1] = false;
        }
    }

    if (p == 3) {
        if (n == 5) {
            retType[0] = true;
            retType[1] = false;
        }
        if (n == 2) {
            obstaclePos = unitMap >> 1 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 8) {
            obstaclePos = unitMap >> 7 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 0 || n == 1 || n == 6 || n == 7) {
            retType[0] = false;
            retType[1] = false;
        }
    }

    if (p == 5) {
        if (n == 3) {
            retType[0] = true;
            retType[1] = false;
        }
        if (n == 0) {
            obstaclePos = unitMap >> 1 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 6) {
            obstaclePos = unitMap >> 7 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }

        if (n == 1 || n == 2 || n == 7 || n == 8) {
            retType[0] = false;
            retType[1] = false;
        }
    }


    if (p == 6) {
        if (n == 1 || n == 2 || n == 5) {
            retType[0] = true;
            retType[1] = false;
        }
        if (n == 0) {
            obstaclePos = unitMap >> 3 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 8) {
            obstaclePos = unitMap >> 7 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }

        if (n == 3 || n == 7) {
            retType[0] = false;
            retType[1] = false;
        }
    }

    if (p == 7) {
        if (n == 1) {
            retType[0] = true;
            retType[1] = false;
        }
        if (n == 0) {
            obstaclePos = unitMap >> 3 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 2) {
            obstaclePos = unitMap >> 5 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 3 || n == 5 || n == 6 || n == 8) {
            retType[0] = false;
            retType[1] = false;
        }

    }

    if (p == 8) {
        if (n == 0 || n == 1 || n == 3) {
            retType[0] = true;
            retType[1] = false;
        }
        if (n == 2) {
            obstaclePos = unitMap >> 5 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 6) {
            obstaclePos = unitMap >> 7 & 0x01;
            if (1 == obstaclePos) {
                retType[0] = true;
                retType[1] = true;
            }
            if (0 == obstaclePos) {
                retType[0] = false;
                retType[1] = false;
            }
        }
        if (n == 5 || n == 7) {
            retType[0] = false;
            retType[1] = false;
        }
    }

    return retType;
}

void Sfjps::Init(int** _map, int _height, int _width) {

    //初始化空节点
    nullNode.isnull = true;

    height = _height;
    width = _width;

    //建立辅助地图
    pathMap = new PathNode * *[height];
    for (int i = 0; i < height; i++) {
        pathMap[i] = new PathNode * [width];
        for (int j = 0; j < width; j++) {
            pathMap[i][j] = new PathNode;
            memset(pathMap[i][j], 0, sizeof(PathNode));
            pathMap[i][j]->row = i;
            pathMap[i][j]->col = j;
            pathMap[i][j]->isfind = false;
            pathMap[i][j]->isroute = false;
            pathMap[i][j]->value = _map[i][j];
        }
    }

}



//直跳跃
//入参：辅助地图、当前节点、直跳跃方向-x方向值，y方向值
//返回跳点
Sfjps::PathNode Sfjps::JumpStraight(PathNode*** _pathMap, PathNode currenNode, Direct dir) {
    int delta_x = 0;
    int delta_y = 0;
    short unitMap = 0;
    char valueT = 0;//存储辅助地图中点的临时值，用于算出单元地图值
    bool* nodeTyp;
    char parent;//单元地图中，父节点
    char neighbGroup[9] = { 9,9,9,9,9,9,9,9,9 };//单元地图中,要探测的邻居组，初始化为非(0-8)的值，为9的点为不可行点
    switch (dir)
    {
    case p_up:
        delta_x = 0;
        delta_y = -1;
        parent = 7;
        break;
    case p_down:
        delta_x = 0;
        delta_y = 1;
        parent = 1;
        break;
    case p_left:
        delta_x = -1;
        delta_y = 0;
        parent = 5;
        break;
    case p_right:
        delta_x = 1;
        delta_y = 0;
        parent = 3;
        break;
    default:
        break;
    }

    PathNode nodeTmp = currenNode;//沿指定方向搜寻的点
    //沿指定方向搜寻，知道找到跳点，或碰到障碍物，或超出地图
    while (1) {
        nodeTmp.row += delta_y;
        nodeTmp.col += delta_x;
        //cout<<"直跳跃："<<nodeTmp.row<<","<<nodeTmp.col<<endl;
        //如果搜寻到终点就返回
        if (nodeTmp.row == endNode.row &&
            nodeTmp.col == endNode.col) return nodeTmp;
        //如果搜寻点，是障碍物，或者出了地图，返回空
        if (height <= nodeTmp.row || 0 > nodeTmp.row ||
            width <= nodeTmp.col || 0 > nodeTmp.col ||
            1 == _pathMap[nodeTmp.row][nodeTmp.col]->value
            ) return nullNode;

        //获取搜寻点周围3x3单元地图，并找到邻居组
        unitMap = 0;//清空单元地图
        for (int i = 0; i < 9; i++) {//初始化邻居组
            neighbGroup[i] = 9;
        }

        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                int row_t = nodeTmp.row + i - 1;//获取周围的点坐标
                int col_t = nodeTmp.col + j - 1;
                if (height > row_t && 0 <= row_t &&
                    width > col_t && 0 <= col_t
                    ) {//确保周围点没超出地图
                    valueT = _pathMap[row_t][col_t]->value;
                    unitMap = unitMap | valueT << (i * 3 + j);
                    if (1 != valueT) {//不为障碍
                        neighbGroup[i * 3 + j] = (i * 3 + j);
                    }
                }
            }
        }//end-获取搜寻点周围3x3单元地图，并找到邻居组

        //获取当前搜寻点周围点类型
        for (int i = 0; i < 9; i++) {
            if (9 != neighbGroup[i] &&
                parent != neighbGroup[i] &&
                4 != neighbGroup[i]
                ) {//如果邻居组中点不为：空(9)、当前搜寻点(4)、父节点
                nodeTyp = Prune(unitMap, parent, neighbGroup[i]);
                if (1 == nodeTyp[1]) {//如果存在强迫邻居，返回当前搜寻点
                    return nodeTmp;
                }

            }
        }//end-获取当前搜寻点周围点类型


    }//while(o)-end
}


Sfjps::PathNode Sfjps::JumpOblique(PathNode*** _pathMap, PathNode currenNode, Direct dir) {
    int delta_x = 0;
    int delta_y = 0;
    short unitMap = 0;
    char valueT = 0;//存储辅助地图中点的临时值，用于算出单元地图值
    bool* nodeTyp;
    char parent;//单元地图中，父节点
    char neighbGroup[9] = { 9,9,9,9,9,9,9,9,9 };//单元地图中,要探测的邻居组，初始化为非(0-8)的值，为9的点为不可行点
    Direct straightDirs[2] = { p_up,p_up };
    switch (dir)
    {
    case p_leftup:
        delta_x = -1;
        delta_y = -1;
        parent = 8;
        straightDirs[0] = p_left;
        straightDirs[1] = p_up;
        break;
    case p_leftdown:
        delta_x = -1;
        delta_y = 1;
        parent = 2;
        straightDirs[0] = p_left;
        straightDirs[1] = p_down;
        break;
    case p_rightup:
        delta_x = 1;
        delta_y = -1;
        parent = 6;
        straightDirs[0] = p_right;
        straightDirs[1] = p_up;
        break;
    case p_rightdown:
        delta_x = 1;
        delta_y = 1;
        parent = 0;
        straightDirs[0] = p_right;
        straightDirs[1] = p_down;
        break;
    default:
        break;
    }

    PathNode nodeTmp = currenNode;//沿指定方向搜寻的点
    //沿指定方向搜寻，知道找到跳点，或碰到障碍物，或超出地图
    while (1) {
        nodeTmp.row += delta_y;
        nodeTmp.col += delta_x;
        //cout<<"斜跳跃："<<nodeTmp.row<<","<<nodeTmp.col<<endl;
        //如果搜寻到终点就返回
        if (nodeTmp.row == endNode.row &&
            nodeTmp.col == endNode.col) return nodeTmp;
        //如果搜寻点，是障碍物，或者出了地图，返回空
        if (height <= nodeTmp.row || 0 > nodeTmp.row ||
            width <= nodeTmp.col || 0 > nodeTmp.col ||
            1 == _pathMap[nodeTmp.row][nodeTmp.col]->value
            ) return nullNode;

        //获取搜寻点周围3x3单元地图，并找到邻居组
        unitMap = 0;//清空单元地图
        for (int i = 0; i < 9; i++) {//初始化邻居组
            neighbGroup[i] = 9;
        }

        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                int row_t = nodeTmp.row + i - 1;//获取周围的点坐标
                int col_t = nodeTmp.col + j - 1;
                if (height > row_t && 0 <= row_t &&
                    width > col_t && 0 <= col_t
                    ) {//确保周围点没超出地图
                    valueT = _pathMap[row_t][col_t]->value;
                    unitMap = unitMap | valueT << (i * 3 + j);
                    if (1 != valueT) {//不为障碍
                        neighbGroup[i * 3 + j] = (i * 3 + j);
                    }
                }
            }
        }//end-获取搜寻点周围3x3单元地图，并找到邻居组

        //获取当前搜寻点周围点类型，如果存在强迫邻居，返回当前搜寻点
        for (int i = 0; i < 9; i++) {
            if (9 != neighbGroup[i] &&
                parent != neighbGroup[i] &&
                4 != neighbGroup[i]
                ) {//如果邻居组中点不为：空(9)、当前搜寻点(4)、父节点
                nodeTyp = Prune(unitMap, parent, neighbGroup[i]);
                if (1 == nodeTyp[1]) {//如果存在强迫邻居，返回当前搜寻点
                    return nodeTmp;
                }

            }
        }//end-获取当前搜寻点周围点类型

        //往当前点的直线“真。邻居“，做直跳跃，如果不反回空，返回当前点
        PathNode jumpNode;//用于保存直跳跃的返回节点
        for (int i = 0; i < 2; i++) {
            jumpNode = JumpStraight(_pathMap, nodeTmp, straightDirs[i]);
            if (false == jumpNode.isnull) return nodeTmp;
        }


    }
}


vector<Sfjps::PathNode> Sfjps::FindPath(PathNode _startNode, PathNode _endNode) {
    //设置开始结束点
    startNode = _startNode;
    endNode = _endNode;

    vector<Direct> jumpDirs;//存放当前需要跳跃的方向
    vector<Direct>::iterator dirsIt;//用于检索反向树的迭代器
    PathNode jumpNode;//返回的跳点
    bool* nodeTyp;//返回的邻居类型

    PathNode currentNode;//当前节点
    vector<PathNode> openTree;//开放列表，关闭列表是用辅助地图各点的isfind属性维护的
    vector<PathNode>::iterator it;//用于迭代
    vector<PathNode>::iterator minF_iter;//存放最小f值节点

    currentNode = startNode;//当前点为开始点
    pathMap[currentNode.row][currentNode.col]->isfind = true;

    //初始方向树（八个方向）
    for (int i = 0; i < 8; i++) {
        jumpDirs.push_back((Direct)i);
    }

    //寻路
    while (1) {

        //利用当前点，以及parent方向，往所有“真。邻居“方向跳跃
        for (dirsIt = jumpDirs.begin(); dirsIt != jumpDirs.end(); dirsIt++) {
            //cout<<"方向："<<(*dirsIt)<<" "<<endl;
            if (*(dirsIt) == p_up || *(dirsIt) == p_down || *(dirsIt) == p_left || *(dirsIt) == p_right) {
                jumpNode = JumpStraight(pathMap, currentNode, (*dirsIt));
            }
            if (*(dirsIt) == p_leftup || *(dirsIt) == p_leftdown || *(dirsIt) == p_rightup || *(dirsIt) == p_rightdown) {
                jumpNode = JumpOblique(pathMap, currentNode, (*dirsIt));
            }

            //如果返回的是有效节点，且不在关闭列表中（未找过）
            if (false == jumpNode.isnull && false == pathMap[jumpNode.row][jumpNode.col]->isfind) {

                jumpNode.g = pathMap[currentNode.row][currentNode.col]->g + GetDis(currentNode, jumpNode);
                //如果该点已经在开放列表中，比较g值，取g值较小的点的属性，并不用再次加入开放列表
                if (pathMap[jumpNode.row][jumpNode.col]->inopen) {
                    if (pathMap[jumpNode.row][jumpNode.col]->g > jumpNode.g) {
                        pathMap[jumpNode.row][jumpNode.col]->g = jumpNode.g;
                        pathMap[jumpNode.row][jumpNode.col]->GetF();

                        pathMap[jumpNode.row][jumpNode.col]->parent = pathMap[currentNode.row][currentNode.col];
                    }

                }
                //如果不在开放列表中
                if (false == pathMap[jumpNode.row][jumpNode.col]->inopen) {
                    jumpNode.h = GetH(jumpNode, endNode);
                    jumpNode.GetF();
                    jumpNode.inopen = true;

                    //将探测点加入开放列表
                    openTree.push_back(jumpNode);
                    //更新辅助地图中对应探测点的节点属性
                    pathMap[jumpNode.row][jumpNode.col]->g = jumpNode.g;
                    pathMap[jumpNode.row][jumpNode.col]->h = jumpNode.h;
                    pathMap[jumpNode.row][jumpNode.col]->f = jumpNode.f;
                    pathMap[jumpNode.row][jumpNode.col]->parent = pathMap[currentNode.row][currentNode.col];
                    pathMap[jumpNode.row][jumpNode.col]->inopen = jumpNode.inopen;
                }


                //system("pause");

            }


        }

        if (openTree.size() == 0) break;
        //找下一点
        minF_iter = openTree.begin();
        //cout<<endl<<"找下一点"<<endl;
        for (it = openTree.begin(); it != openTree.end(); it++) {
            //cout<<(*it).row<<","<<(*it).col<<endl;
            if (pathMap[(*it).row][(*it).col]->f < pathMap[(*minF_iter).row][(*minF_iter).col]->f) {
                minF_iter = it;
            }
        }

        //cout<<endl<<"找到的下一点: ";
        //cout<<(*minF_iter).row<<","<<(*minF_iter).col<<endl;

#if 0   //调试
        cout << endl << "找到的下一点: ";
        cout << (*minF_iter).row << "," << (*minF_iter).col << endl;
        cout << "此节点父节点坐标：";
        PathNode tmp = { (*minF_iter).row,(*minF_iter).col };
        while (NULL != pathMap[tmp.row][tmp.col]->parent) {
            int t_row = tmp.row, t_col = tmp.col;
            tmp.row = pathMap[t_row][t_col]->parent->row;
            tmp.col = pathMap[t_row][t_col]->parent->col;
            cout << tmp.row << "," << tmp.col << " ";
        }
        cout << endl;
#endif


        currentNode = (*minF_iter);

        pathMap[currentNode.row][currentNode.col]->isfind = true;

        if (currentNode.row == endNode.row && currentNode.col == endNode.col) break;

        openTree.erase(minF_iter);

        //获取当前节点即将要搜寻的方向，jumpDirs
        jumpDirs.clear();
        int delta_y = currentNode.row - pathMap[currentNode.row][currentNode.col]->parent->row;
        int delta_x = currentNode.col - pathMap[currentNode.row][currentNode.col]->parent->col;
        char p;//单元地图中父点
        short unitMap = 0;
        char neighbGroup[9] = { 9,9,9,9,9,9,9,9,9 };//单元地图中,要探测的邻居组，初始化为非(0-8)的值，为9的点为不可行点

        if (0 > delta_y && 0 == delta_x) p = 7;//搜寻方向为上
        if (0 < delta_y && 0 == delta_x) p = 1;//搜寻方向为下
        if (0 == delta_y && 0 > delta_x) p = 5;//搜寻方向为左
        if (0 == delta_y && 0 < delta_x) p = 3;//搜寻方向为右

        if (0 > delta_y && 0 > delta_x) p = 8;//搜寻方向为左上
        if (0 < delta_y && 0 > delta_x) p = 2;//搜寻方向为左下
        if (0 > delta_y && 0 < delta_x) p = 6;//搜寻方向为右上
        if (0 < delta_y && 0 < delta_x) p = 0;//

        //获取搜寻点周围3x3单元地图，并找到邻居组

        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                int row_t = currentNode.row + i - 1;//获取周围的点坐标
                int col_t = currentNode.col + j - 1;
                if (height > row_t && 0 <= row_t &&
                    width > col_t && 0 <= col_t
                    ) {//确保周围点没超出地图
                    int valueT = pathMap[row_t][col_t]->value;
                    unitMap = unitMap | valueT << (i * 3 + j);
                    if (1 != valueT) {//不为障碍
                        neighbGroup[i * 3 + j] = (i * 3 + j);
                    }
                }
            }
        }//end-获取搜寻点周围3x3单元地图，并找到邻居组

        //获取当前搜寻点周围点类型，并赋值探测方向组
        for (int i = 0; i < 9; i++) {
            if (9 != neighbGroup[i] &&
                p != neighbGroup[i] &&
                4 != neighbGroup[i]
                ) {//如果邻居组中点不为：空(9)、当前搜寻点(4)、父节点
                nodeTyp = Prune(unitMap, p, neighbGroup[i]);
                if (1 == nodeTyp[0]) {//如果存在关键邻居，就向探测方向组中加入当前方向
                    if (1 == i) jumpDirs.push_back(p_up);
                    if (7 == i) jumpDirs.push_back(p_down);
                    if (3 == i) jumpDirs.push_back(p_left);
                    if (5 == i) jumpDirs.push_back(p_right);

                    if (0 == i) jumpDirs.push_back(p_leftup);
                    if (6 == i) jumpDirs.push_back(p_leftdown);
                    if (2 == i) jumpDirs.push_back(p_rightup);
                    if (8 == i) jumpDirs.push_back(p_rightdown);

                }

            }
        }//end-获取当前搜寻点周围点类型，并赋值探测方向组

        //system("pause");

    }

    //返回路径
    retPath.clear();
    PathNode nodeTmp = endNode;
    while (1) {
        int row_t = nodeTmp.row;
        int col_t = nodeTmp.col;
        retPath.push_back(nodeTmp);
        pathMap[row_t][col_t]->isroute = true;
        if (NULL == pathMap[nodeTmp.row][nodeTmp.col]->parent) break;
        nodeTmp.row = pathMap[row_t][col_t]->parent->row;
        nodeTmp.col = pathMap[row_t][col_t]->parent->col;
    }
    reverse(retPath.begin(), retPath.end());

    ////将路径整理为从开始点出发的顺序
    retPath1.clear();
    PathNode tmp;
    tmp = { retPath[0].row,retPath[0].col };
    retPath1.push_back(tmp);
    for (int i = 0; i < retPath.size() - 2; i++) {
        //tmp1 = { retPath[i + 2].row,retPath[i + 2].col };
        if (!isLineOfSight(retPath[i], retPath[i + 2])) {
            tmp = { retPath[i + 1].row,retPath[i + 1].col };
            retPath1.push_back(tmp);
        }
    }
    tmp = { retPath[retPath.size() - 1].row,retPath[retPath.size() - 1].col };
    retPath1.push_back(tmp);
    return retPath1;
}

// 新增函数，设置安全跳点
void Sfjps::setSafeJumpPoint(PathNode& node, PathNode& motion) {
    // 这里假设node是当前节点，motion是要执行的动作对应的节点
    // 检测是否为对角运动
    //if (motion.col != 0 && motion.row != 0) {
    //    // 如果存在跳点在水平或垂直方向
    //    PathNode x_dir = { motion.col, 0, 0, 0, 0, false, false, false, nullptr };
    //    PathNode y_dir = { 0, motion.row, 0, 0, 0, false, false, false, nullptr };
    //    if (JumpStraight(pathMap, node, static_cast<Direct>(3)) || JumpStraight(pathMap, node, static_cast<Direct>(1))) {
    //        // 手动处理每个元素，示例中这里假设设置为当前节点和父节点的某种平均位置
    //        node.col = (node.col + node.parent->col) / 2;
    //        node.row = (node.row + node.parent->row) / 2;
    //        node.parent = node.parent;
    //    }
    //    else if (JumpStraight(pathMap, node, static_cast<Direct>(2)) || JumpStraight(pathMap, node, static_cast<Direct>(0))) {
    //        // 手动处理每个元素，示例中这里假设设置为当前节点和父节点的某种平均位置
    //        node.col = 2 * node.col - node.parent->col;
    //        node.row = 2 * node.row - node.parent->row;
    //        node.parent = node.parent;
    //    }
    //}
    // 这里可以根据具体需求进一步完善安全跳点的设置逻辑，比如考虑更多的情况和约束条件
}

// 新增函数，根据角度确定搜索方向的优先级
void Sfjps::determinePriority(double theta, vector<Direct>& priority) {
    if (0 < theta && theta <= M_PI / 2) {
        priority = { Direct::p_right, Direct::p_up, Direct::p_rightup, Direct::p_rightdown, Direct::p_down, Direct::p_leftdown, Direct::p_left, Direct::p_leftup };
    }
    else if (M_PI / 2 < theta && theta <= M_PI) {
        priority = { Direct::p_up, Direct::p_left, Direct::p_leftup, Direct::p_rightup, Direct::p_right, Direct::p_rightdown, Direct::p_down, Direct::p_leftdown };
    }
    else if (-M_PI < theta && theta <= -M_PI / 2) {
        priority = { Direct::p_left, Direct::p_down, Direct::p_leftdown, Direct::p_leftup, Direct::p_up, Direct::p_rightup, Direct::p_right, Direct::p_rightdown };
    }
    else {
        priority = { Direct::p_down, Direct::p_right, Direct::p_rightdown, Direct::p_leftdown, Direct::p_left, Direct::p_leftup, Direct::p_up, Direct::p_rightup };
    }
}

// 新增函数，检测是否存在强迫邻居并处理
bool Sfjps::detectForceNeighbor(PathNode& node, PathNode& motion) {
    int x = node.col;
    int y = node.row;
    int x_dir = motion.col;
    int y_dir = motion.row;

    // 水平方向
    //if (x_dir != 0 && y_dir == 0) {
    //    if ((x, y + 1) in pathMap && pathMap[(x, y + 1)]->value == 1 &&
    //        (x + x_dir, y + 1) in pathMap && pathMap[(x + x_dir, y + 1)]->value != 1) {
    //        return true;
    //    }
    //    if ((x, y - 1) in pathMap && pathMap[(x, y - 1)]->value == 1 &&
    //        (x + x_dir, y - 1) in pathMap && pathMap[(x + x_dir, y - 1)]->value != 1) {
    //        return true;
    //    }
    //}

    //// 垂直方向
    //if (x_dir == 0 && y_dir != 0) {
    //    if ((x + 1, y) in pathMap && pathMap[(x + 1, y)]->value == 1 &&
    //        (x + 1, y + y_dir) in pathMap && pathMap[(x + 1, y + y_dir)]->value != 1) {
    //        return true;
    //    }
    //    if ((x - 1, y) in pathMap && pathMap[(x - 1, y)]->value == 1 &&
    //        (x - 1, y + y_dir) in pathMap && pathMap[(x - 1, y + y_dir)]->value != 1) {
    //        return true;
    //    }
    //}

    // 对角方向
    //if (x_dir != 0 && y_dir != 0) {
    //    if ((x - x_dir, y) in pathMap && pathMap[(x - x_dir, y)]->value == 1 &&
    //        (x - x_dir, y + y_dir) in pathMap && pathMap[(x - x_dir, y + y_dir)]->value != 1) {
    //        return true;
    //    }
    //    if ((x, y - y_dir) in pathMap && pathMap[(x, y - y_dir)]->value == 1 &&
    //        (x + x_dir, y - y_dir) in pathMap && pathMap[(x + x_dir, y - y_dir)]->value != 1) {
    //        return true;
    //    }
    //}

    return false;
}

// 辅助函数，判断两点之间是否存在视线通廊（直线无障碍）
bool Sfjps::isLineOfSight(PathNode start, PathNode end) {
    int x0 = start.col;
    int y0 = start.row;
    int x1 = end.col;
    int y1 = end.row;
    int dx = abs(x1 - x0);
    int dy = abs(y1 - y0);
    int sx = 1;
    if (x0 > x1) sx = -1;
    int sy = 1;
    if (y0 > y1) sy = -1;
    int err = dx - dy;

    while (true) {
        for (int x = min(x0, x1); x <= max(x0, x1)+1; x++) {
            for (int y = min(y0, y1); y <= max(y0, y1)+1; y++) {
                if (pathMap[x][y]->value == 1) {
                    return false;
                }
            }
        }
        if (x0 == x1 && y0 == y1) {
            break;
        }
        int e2 = 2 * err;
        if (e2 > -dy) {
            err -= dy;
            x0 += sx;
        }
        if (e2 < dx) {
            err += dx;
            y0 += sy;
        }
    }
    return true;
}

// 计算两个点之间的差值
vector<Sfjps::PathNode*> Sfjps::interpolate(PathNode start, PathNode end) {
    vector<PathNode*> result;
    int dx = end.row - start.row; // 修正为使用 start.row
    int dy = end.col - start.col; // 修正为使用 start.col

    int steps = max(abs(dx), abs(dy)); // 差值步骤数量

    for (int i = steps; i > 0; i--) {
        PathNode* nodeTemp = new PathNode;
        nodeTemp->row = start.row + dx * (steps - i) / steps;
        nodeTemp->col = start.col + dy * (steps - i) / steps;
        // 这里也要确保 newX 和 newY 在有效范围内
        if (nodeTemp->row >= 0 && nodeTemp->col >= 0) {
            // pathMap[newX][newY]->isroute = true;
            result.push_back(nodeTemp);
        }
    }
    return result;
}

void Sfjps::PrintRoute() {
    vector<PathNode>::iterator it;//用于迭代
    float routLength = 0;//路径总长度
    cout << endl << "route" << "(" << retPath.size() << "): ";
    for (it = retPath.begin(); it != retPath.end(); it++) {
        cout << (*it).row << "," << (*it).col << " ";
        //计算路径长度
        if (it > retPath.begin()) {
            int row_t = (*it).row, col_t = (*it).col;//本次坐标
            int row_t_l = (*(it - 1)).row, col_t_l = (*(it - 1)).col;//上次坐标
            routLength += sqrt(pow(col_t - col_t_l, 2) + pow((row_t - row_t_l), 2));//pow次方函数
        }
    }
    cout << endl;
    cout << "routLength：" << routLength;

}

void Sfjps::PrintRouteMap() {

    //打印路线地图
    cout << endl;
    for (int i = 0; i < height; i++) {
        for (int j = 0; j < width; j++) {
            if (pathMap[i][j]->isroute)
                cout << "*";
            else cout << pathMap[i][j]->value;
        }
        cout << endl;
    }

}